#include "mqtt.h"

// 设置此位后 客户端将不会接收到旧的应用信息
#define MQTT_CONNECT_CLEAN_SESSION 	0b00000010
// 设置此位后 客户端要求非正常情况下退出触发遗嘱 客户端正常流程退出不会触发遗嘱
#define MQTT_CONNECT_WILL_FLAG		0b00000100
// 设置遗嘱等级为Qos1
#define MQTT_CONNECT_WILL_FLAG_QOS1	0b00001000
// 设置遗嘱等级为Qos2
#define MQTT_CONNECT_WILL_FLAG_QOS2	0b00010000
// 设置此位后 客户端要求服务端保留客户端遗嘱
#define MQTT_CONNECT_WILL_RETAIN	0b00100000
// 设置此位后 客户端本次CONNECT带有密码
#define MQTT_CONNECT_PASSWORD		0b01000000
// 设置此位后 客户端本次CONNECT带有用户名
#define MQTT_CONNECT_USERNAME 		0b10000000

/// @brief 计算长度占mqtt长度位
/// @param len 
/// @return 
static u8 mqtt_len_level_handle(u64 len)
{
	u8 level = 1;
	static const u32 mqtt_len_level[4] = { 127,16383,2097151,268435455 };
	for (size_t i = 0; i < 4; i++)
	{
		if (len > mqtt_len_level[i])
			level++;
		else
			break;
	}
	return level;
}

// 固定报头（1） + 4字节长度（1-4） + 可变报头 + 负载

/// @brief mqtt connect 连接打包
/// @param client_id 客户端id
/// @param username mqtt客户端用户名
/// @param password mqtt客户端密码
/// @param alive_time 保活时间
/// @param clean_session 客户端不接收旧应用信息
/// @param pack_len  回填 数据包长度 
/// @return 
u8 *mqtt_connect_packet(
	_IN const u8 *client_id,
	_IN const u8 *username,
	_IN const u8 *password,
	_IN const u16 alive_time,
	_IN const u8 clean_session,
	_OUT u64 *pack_len)
{

	u64 client_id_len = client_id ? mqtt_strlen(client_id) : 0;
	u64 username_len = username ? mqtt_strlen(username) : 0;
	u64 password_len = password ? mqtt_strlen(password) : 0;

	u8 var_header[10] = {
		0x00,0x04, // len
		'M','Q','T','T', // MQTT
		0x04, // 协议版本
	}; // 7 + flag(1位) + keep alive time (2位)

	u64 payload_len = 2 + client_id_len;
	u8 flag = 0;
	if (username_len != 0)
	{
		payload_len += 2 + username_len;
		flag |= MQTT_CONNECT_USERNAME;
	}

	if (password_len != 0)
	{
		payload_len += 2 + password_len;
		flag |= MQTT_CONNECT_PASSWORD;
	}
	if (clean_session)
		flag |= MQTT_CONNECT_CLEAN_SESSION;

	var_header[7] = flag;
	var_header[8] = alive_time >> 8;
	var_header[9] = alive_time & 0xff;

	// 可变报头+负载
	u64 remain_len = sizeof(var_header) + payload_len;

	// 4字节u8数组(数据长度)编码
	u8 len_level = mqtt_len_level_handle(remain_len);
	// 数据缓存区
	u8 *pack = (u8 *)mqtt_malloc(1 + len_level + remain_len);
	if (pack == NULL)
		return NULL;
	mqtt_memset(pack, 0,1 + len_level + remain_len);
	u64 pack_offset = 0;
	pack[pack_offset++] = 0x10; // 固定报头

	do
	{
		u8 encodedByte = remain_len % 128;
		remain_len = remain_len / 128;
		if (remain_len > 0)
			encodedByte = encodedByte | 128;
		pack[pack_offset++] = encodedByte;
	} while (remain_len > 0);
	// 可变报头
	mqtt_memcpy(pack+pack_offset, var_header, sizeof(var_header));
	pack_offset += (sizeof(var_header) / sizeof(u8));

	// 负载
	pack[pack_offset++] = client_id_len >> 8;
	pack[pack_offset++] = client_id_len & 0xff;
	mqtt_memcpy(pack+pack_offset, client_id, client_id_len);
	pack_offset += client_id_len;

	if (username_len != 0)
	{
		pack[pack_offset++] = username_len >> 8;
		pack[pack_offset++] = username_len & 0xff;
		mqtt_memcpy(pack+pack_offset, username, username_len);
		pack_offset += username_len;
	}


	if (password_len != 0)
	{
		pack[pack_offset++] = password_len >> 8;
		pack[pack_offset++] = password_len & 0xff;
		mqtt_memcpy(pack+pack_offset, password, password_len);
		pack_offset += password_len;
	}
	if (pack_len != NULL)
		*pack_len = pack_offset;
	return pack;
}

// 此位开启表示要求服务器保留该信息,未来若有客户端订阅该主题则分发该数据
#define MQTT_PUBLISH_RETAIN	0b00000001
// qos1级 至少分发一次
#define MQTT_PUBLISH_QOS1 	0b00000010
// qos2级 只分发一次
#define MQTT_PUBLISH_QOS2 	0b00000100
// 此位开启表示该信息已发送过一次
#define MQTT_PUBLISH_DUP	0b00001000

/// @brief mqtt publish 发布打包
/// @param qos 发布等级
/// @param topic 标题
/// @param data 数据
/// @param pack_len 回填 数据包长度
/// @param seq 消息id
/// @param dup 是否重发
/// @param retain 是否要求服务器记录
/// @return 
u8 *mqtt_publish_packet(
	_IN const u8 qos,
	_IN const u8 *topic,
	_IN const u8 *data,
	_OUT u64 *pack_len,
	_IN u16 seq,
	_IN u8 dup,
	_IN u8 retain)
{
	u64 topic_len = topic ? mqtt_strlen(topic) : 0;
	u64 data_len = data ? mqtt_strlen(data) : 0;
	if (topic_len == 0 || data_len == 0)
		return NULL;
	u8 qos_flag = 0;
	u64 qos_len = 0;
	u8 head = 0x30;
	if (qos < 3)
	{
		if (qos != 0)
		{
			qos_len = 2;
			if (qos == 1)
				qos_flag = MQTT_PUBLISH_QOS1;
			else
				qos_flag = MQTT_PUBLISH_QOS2;
		}
		head |= qos_flag;
	}
	else
	{
		return NULL;
	}
	if (dup)
		head |= MQTT_PUBLISH_DUP;
	if (retain)
		head |= MQTT_PUBLISH_RETAIN;
	
	u64 var_header_len = topic_len + 2 + qos_len;
	u64 remain_len = var_header_len + data_len;

	u8 len_level = mqtt_len_level_handle(remain_len);
	
	u64 pack_offset = 0;
	u8 *pack = mqtt_malloc(1+ len_level + remain_len);
	if (pack == NULL)
		return NULL;
	mqtt_memset(pack, 0, 1 + len_level + remain_len);
	// 固定报头
	pack[pack_offset++] = head;
	// 4字节长度
	do
	{
		u8 encodedByte = remain_len % 128;
		remain_len = remain_len / 128;
		if (remain_len > 0)
			encodedByte = encodedByte | 128;
		pack[pack_offset++] = encodedByte;
	} while (remain_len > 0);

	// 可变报头
	pack[pack_offset++] = topic_len >> 8;
	pack[pack_offset++] = topic_len & 0xff;
	mqtt_memcpy(pack + pack_offset, topic, topic_len);
	pack_offset += topic_len;
	// 可变报头Qos qos>0时 此处还需要填入部分信息（所谓的报文标识符）
	if (qos_len)
	{
		pack[pack_offset++] = seq >> 8;
		pack[pack_offset++] = seq & 0xff;
	}
	// 负载
	// pack[pack_offset++] = data_len >> 8;
	// pack[pack_offset++] = data_len >> 0xff;
	mqtt_memcpy(pack + pack_offset, data, data_len);
	pack_offset += data_len;
	if (pack_len != NULL)
		*pack_len = pack_offset;
	return pack;
}


/// @brief mqtt publish qos1 响应打包
/// @param u8_4_buf 回填 u8数组
/// @param seq 消息id
void mqtt_puback_packet(_OUT u8 *u8_4_buf, _IN u16 seq)
{
	if (u8_4_buf == NULL)
		return;
	u8 idx = 0;
	u8_4_buf[idx++] = 0x40;
	u8_4_buf[idx++] = 0x02;
	u8_4_buf[idx++] = seq >> 8;
	u8_4_buf[idx++] = seq & 0xff;
}

/// @brief mqtt publish qos2 响应第一步打包
/// @param u8_4_buf 回填 u8数组
/// @param seq 消息id
void mqtt_pubrec_packet(_OUT u8 *u8_4_buf, _IN u16 seq)
{
	if (u8_4_buf == NULL)
		return;
	u8 idx = 0;
	u8_4_buf[idx++] = 0x50;
	u8_4_buf[idx++] = 0x02;
	u8_4_buf[idx++] = seq >> 8;
	u8_4_buf[idx++] = seq & 0xff;
}

/// @brief mqtt publish qos2 pubrec响应后回发打包
/// @param u8_6_buf 回填 u8数组
/// @param seq 消息id
void mqtt_pubrel_packet(_OUT u8 *u8_6_buf, _IN u16 seq)
{
	if (u8_6_buf == NULL)
		return;
	u8 idx = 0;
	u8_6_buf[idx++] = 0x62;
	u8_6_buf[idx++] = 0x02;
	u8_6_buf[idx++] = seq >> 8;
	u8_6_buf[idx++] = seq & 0xff;
	u8_6_buf[idx++] = 0x00;
	u8_6_buf[idx++] = 0x00;
}

/// @brief mqtt publish qos2 pubrel回发后响应打包
/// @param u8_4_buf 回填 u8数组
/// @param seq 消息id
void mqtt_pubcomp_packet(_OUT u8 *u8_4_buf, _IN u16 seq)
{
	if (u8_4_buf == NULL)
		return;
	u8 idx = 0;
	u8_4_buf[idx++] = 0x70;
	u8_4_buf[idx++] = 0x02;
	u8_4_buf[idx++] = seq >> 8;
	u8_4_buf[idx++] = seq & 0xff;
}

/// @brief mqtt subscribe 订阅打包
/// @param topics 主题数组
/// @param qoss qos数组
/// @param topics_len 数组长度
/// @param pack_len 回填 数据包长度
/// @param seq 消息id
/// @return 
extern u8 *mqtt_subscribe_packet(
	_IN u8 **topics,
	_IN u8 *qoss,
	_IN u64 topics_len,
	_OUT u64 *pack_len,
	_IN u16 seq)
{
	if (topics == NULL || qoss == NULL || topics_len == 0)
		return NULL;
	u8 var_header_len = 2;

	u64 payload_len = 0;
	for (u64 i = 0; i < topics_len; i++)
	{
		u8 *topic = topics[i];
		payload_len += (2 + mqtt_strlen(topic) + 1); // topic_len + qos
	}
	u64  remain_len = var_header_len + payload_len;
	u8 len_level = mqtt_len_level_handle(remain_len);
	u8 *pack = mqtt_malloc(1 + len_level + remain_len);
	if (pack == NULL)
		return NULL;
	u64 pack_offset = 0;
	// 固定报头
	pack[pack_offset++] = 0x82;
	// 1～4字节长度
	do
	{
		u8 encodedByte = remain_len % 128;
		remain_len = remain_len / 128;
		if (remain_len > 0)
			encodedByte = encodedByte | 128;
		pack[pack_offset++] = encodedByte;
	} while (remain_len > 0);

	// 可变报头
	pack[pack_offset++] = seq >> 8;
	pack[pack_offset++] = seq & 0xff;
	// 负载
	for (u64 i = 0; i < topics_len; i++)
	{
		u8 *topic = topics[i];
		u64 topic_len = mqtt_strlen(topic);
		pack[pack_offset++] = topic_len >> 8;
		pack[pack_offset++] = topic_len & 0Xff;
		memcpy(pack + pack_offset, topic, topic_len);
		pack_offset += topic_len;
		u8 qos = qoss[i];
		pack[pack_offset++] = qos;
	}
	if (pack_len != NULL)
		*pack_len = pack_offset;
	return pack;
}


/// @brief mqtt suback 订阅响应 订阅成功与否需要解包查看qos数组值
/// @param qoss qos数组
/// @param len 数组长度
/// @param pack_len 数据包长度 
/// @param seq 消息id
/// @return 
u8 *mqtt_suback_packet(
	_IN u8 *qoss,
	_IN u64 len,
	_OUT u64 *pack_len,
	_IN u16 seq)
{
	if (qoss == NULL || len == 0)
		return NULL;

	u64 var_header_len = 2;
	u64 payload_len = len;
	u64 remain_len = var_header_len + payload_len;
	
	u8 len_level = mqtt_len_level_handle(remain_len);
	u64 pack_offset = 0;
	u8 *pack = mqtt_malloc(1 + len_level + remain_len);
	if (pack == NULL)
		return NULL;
	// 固定报头
	pack[pack_offset++] = 0x90;
	// 1～4字节长度
	do
	{
		u8 encodedByte = remain_len % 128;
		remain_len = remain_len / 128;
		if (remain_len > 0)
			encodedByte = encodedByte | 128;
		pack[pack_offset++] = encodedByte;
	} while (remain_len > 0);
	// 可变报头
	pack[pack_offset++] = seq >> 8;
	pack[pack_offset++] = seq & 0xff;

	// 负载
	for (size_t i = 0; i < len; i++)
	{
		pack[pack_offset++] = qoss[i];
	}
	if (pack_len != NULL)
		*pack_len = pack_offset;
	
	return pack;
}


/// @brief mqtt unsubscribe 取消订阅 打包
/// @param topics 主题数组
/// @param topics_len 数组长度
/// @param pack_len 数据包长度
/// @param seq 消息id
/// @return 
u8 *mqtt_unsubscribe_packet(
	_IN u8 **topics,
	_IN u64 topics_len,
	_OUT u64 *pack_len,
	_IN u16 seq)
{
	if (topics == NULL || topics_len == 0)
		return NULL;

	u64 var_header_len = 2;
	u64 payload_len = 0;
	for (size_t i = 0; i < topics_len; i++)
	{
		payload_len += (2 + mqtt_strlen(topics[i]));
	}
	u64 remain_len = var_header_len + payload_len;
	u8 len_level = mqtt_len_level_handle(remain_len);

	u64 pack_offset = 0;
	u8 *pack = mqtt_malloc(1 + len_level + remain_len);
	if (pack == NULL)
		return NULL;
	// 固定报头
	pack[pack_offset++] = 0xa2;
	// 1～4字节长度
	do
	{
		u8 encodedByte = remain_len % 128;
		remain_len = remain_len / 128;
		if (remain_len > 0)
			encodedByte = encodedByte | 128;
		pack[pack_offset++] = encodedByte;
	} while (remain_len > 0);
	// 可变报头
	pack[pack_offset++] = seq >> 8;
	pack[pack_offset++] = seq & 0xff;
	// 负载
	for (size_t i = 0; i < topics_len; i++)
	{
		u8 *topic = topics[i];
		u64 topic_len = mqtt_strlen(topic);

		pack[pack_offset++] = topic_len >> 8;
		pack[pack_offset++] = topic_len & 0xff;
		memcpy(pack + pack_offset, topic, topic_len);
		pack_offset += topic_len;
	}
	if (pack_len != NULL)
		*pack_len = pack_offset;
	return pack;
}

/// @brief mqtt unsuback 取消订阅打包
/// @param u8_4_buf 
/// @param seq 
void mqtt_unsuback_packet(_OUT u8 *u8_4_buf, _IN u16 seq)
{
	if (u8_4_buf == NULL)
		return;
	u8 idx = 0;
	u8_4_buf[idx++] = 0xb0;
	u8_4_buf[idx++] = 0x02;
	u8_4_buf[idx++] = seq >> 8;
	u8_4_buf[idx++] = seq & 0xff;
}


void mqtt_unpacket_type(
	_IN u8 head,
	_OUT mqtt_pack_type *type)
{
	 if (type == NULL)
        return;
    *type = mqtt_unknow;

    // 只检查 head 变量的前四位
    u8 type_bits = (head & 0xf0) >> 4;
	// 将 head 右移 4 位，然后与 0xF0 进行与操作，以获取前四位
    switch (type_bits) {
        case 0x01:
            *type = mqtt_connect;
            break;
        case 0x02:
            *type = mqtt_connack;
            break;
        case 0x03:
            *type = mqtt_publish;
            break;
        case 0x04:
            *type = mqtt_puback;
            break;
        case 0x05:
            *type = mqtt_pubrec;
            break;
        case 0x06:
            *type = mqtt_pubrel;
            break;
        case 0x07:
            *type = mqtt_pubcomp;
            break;
        case 0x08:
            *type = mqtt_subscribe;
            break;
        case 0x09:
            *type = mqtt_suback;
            break;
        case 0x0a:
            *type = mqtt_unsubscribe;
            break;
        case 0x0b:
            *type = mqtt_unsuback;
            break;
        case 0x0c:
            *type = mqtt_pingreq;
            break;
        case 0x0d:
            *type = mqtt_pingresp;
            break;
        case 0x0e:
            *type = mqtt_disconnect;
            break;
        default:
            *type = mqtt_unknow;
            break;
    }
}


u32 mqtt_decode_remaining_len(_IN const u8 *buffer,_OUT u8 *len_level)
{
	const u8 *ptr = buffer;
    u32 value = 0;
    u32 multiplier = 1;
	u8 encodedByte = 0;
	u8 level = 0;
	do
	{
		encodedByte = *ptr++;
		level++;
		value += (encodedByte & 127) * multiplier;
		multiplier *= 128;
		if (multiplier > 128 * 128 * 128)
			break;
	} while ((encodedByte & 128) != 0);
	if (len_level != NULL)
		*len_level = level;
	return value;
}


void mqtt_unpacket_publish_data(
	_IN u8 head,
	_IN u32 payload_len,
	_IN const u8 *payload_buf,
	_OUT mqtt_publish_data_t *detail)
{
	if (!(head & 0b00110000) || payload_buf == NULL || detail == NULL)
		return;
	
	detail->dup = (head & MQTT_PUBLISH_DUP) ? 1:0;
	
	detail->qos = 0;
	if (head & MQTT_PUBLISH_QOS2)
		detail->qos = 2;
	else if (head & MQTT_PUBLISH_QOS1)
		detail->qos = 1;

	detail->retain = (head & MQTT_PUBLISH_RETAIN) ? 1:0;
	
	u32 payload_buf_offset = 0;
	detail->topic_len = payload_buf[payload_buf_offset++] << 8 | payload_buf[payload_buf_offset++];
	
	detail->topic = (u8*)(payload_buf + payload_buf_offset);
	payload_buf_offset += detail->topic_len;

	detail->seq = detail->qos == 0 ? 0 :payload_buf[payload_buf_offset++] << 8 | payload_buf[payload_buf_offset++];
	detail->data = (u8*)(payload_buf + payload_buf_offset);
}